/*
	This file is part of AVRCppLib.

    AVRCppLib is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    AVRCppLib is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with AVRCppLib.  If not, see <http://www.gnu.org/licenses/>.

    Copyright (C) Maxime Lemonnier 2007-2011
 */

#include "BitManipulation.h"
#include "../Utils/Utils.h"
uint8_t maskToConfig(uint8_t mask)
{
	uint8_t config = 0;

	for(size_t i = 0; i < 8; i++)
		if(bitIsSet(mask, i))
			config++;

	return config;
}

void setConfig(const IOAddress & reg, uint8_t configBitMask, uint8_t config)
{

	if (configBitMask == 0)
		return;

	*reg &= (uint8_t)~configBitMask; // clears all bits in the mask


	uint8_t shiftedConfig = 0x00;

	size_t configIndex = 0;
	for(size_t configBitMaskIndex = 0; configBitMaskIndex < 8; configBitMaskIndex++)
		if( bitIsSet(configBitMask, configBitMaskIndex))
			{
				changeBitValueTo(shiftedConfig, configBitMaskIndex, bitIsSet(config, configIndex)); // sets is a bit in position configBitMaskIndex if the responding bit in config is one
				configIndex++;
			}

	*reg |= shiftedConfig; // write the setting

}


uint8_t getConfig(IOAddress & reg, uint8_t configBitMask)
{

	if (configBitMask == 0)
		return 0;

	uint8_t maskLSB = 0;

	while ((configBitMask >> maskLSB) % 2 == 0) // we shift until we find the mask LSB
		maskLSB++;

	return (*reg & configBitMask) >> maskLSB;
}
